// v1.0 (22/10/14): created to demonstrate velocity control of the robot, and in XYZ mode

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>

#include "MatWork.hpp"

#include "DENSOrobot.hpp"


using namespace cv;
using namespace std;

enum InputMode {READ_PENDANT, INV_KINE };


int run_RS232 = 1;  //0: NO; 1: YES
int mode = CTRL_MODE_XYZ;
InputMode ipm = READ_PENDANT;


int callXYZPlanner(DENSOrobot& DSrb)
{
	int ret = 1;

	Mat OrgPoseJnt;

	// ********  Test TrajPlanner
	printf("*************************\n");
	printf("Trajectory Planner Test\n");

	// define original joint pose from XYZ pose
	// this can be done by invkine or by reading the teaching pendant	
	if (ipm == INV_KINE)
	{
		printf("mode not supported!");
		getchar();
		ret = 0;
		return ret;
		
	} else
	{
		// READ_PENDANT mode
		// {175.0, 0.0, 536.0, -CV_PI, CV_PI/2, CV_PI} XYZ mode <=> {0, -30.08, 120.28, 0, -0.2, 0} joint mode
		double OrgPose_ar[6] = {175.0, 0.0, 536.0, -CV_PI, CV_PI/2, CV_PI};   // manually enter values here
		Mat OrgPose = Mat(6, 1, CV_64F, OrgPose_ar);		  // pose should be in 6x1 format
		Mat OrgPoseJnt;
		OrgPose.copyTo(OrgPoseJnt);
	}

	// define destination pose in XYZ pose
	double DstPose_ar[6] = {150.0, 50.0, 480.0, 0, CV_PI/2, 0};
	Mat DstPose = Mat(6, 1, CV_64F, DstPose_ar);			// pose should be in 6x1 format
	Mat traj;

	DSrb.inputPose(OrgPoseJnt, CTRL_MODE_XYZ);  // check this func
	ret = DSrb.TrajPlanner(OrgPoseJnt, DstPose, traj, CTRL_MODE_XYZ);

	//MATmtxdisp(DSrb.CPose);
	
	return ret;
}


int main()
{
    int ret;
		
	DENSOrobot DSrb = DENSOrobot(run_RS232);
			
	// Perform first motion to ready position
	ret = callXYZPlanner(DSrb);

	printf("Current XYZ pose is:\n");
	MATmtxdisp(DSrb.CPose);
	//getchar();


	// Start to run Velocity control
	int count = 100;
	Mat VelScrew = Mat::zeros(6,1,CV_64FC1);
	while (count > 0)
	{
		count--;
		
		VelScrew.at<double>(0) = 0.1;
		VelScrew.at<double>(1) = 0.1;
		VelScrew.at<double>(2) = 0.1;
		VelScrew.at<double>(3) = 0.1;
		VelScrew.at<double>(4) = 0.1;
		VelScrew.at<double>(5) = 0.1;

		DSrb.VelCtrl(VelScrew, CTRL_MODE_XYZ);

		MATmtxdisp(DSrb.CPose);
	}

	getchar();
	
	return ret;
}
